#ifndef ARM_TASK_H
#define ARM_TASK_H
#include "main.h"

#include "MOTOR_receive.h"
#include "pid.h"
#include "remote_control.h"
//因为每个电机所处的机械臂位置不一样，所处环境不同，故而每个关节使用独立的PID参数而不是相同电机使用同一组参数。

//joint1 达妙电机使用电机控制逻辑，位置模式PD控制
#define JOINT1_DM4310_KP 20.0f
#define JOINT1_DM4310_KD 2.0f
//JOINT2 宇树电机使用电机控制逻辑，位置模式PD控制
#define JOINT2_M8010_KP 0.8
#define JOINT2_M8010_KD 0.05
//JOINT3 宇树电机使用电机控制逻辑，位置模式PD控制
#define JOINT3_M8010_KP 0.8
#define JOINT3_M8010_KD 0.05
//JOINT4 达妙电机使用电机控制逻辑，位置模式PD控制
#define JOINT4_DM4310_KP 20.0f
#define JOINT4_DM4310_KD 2.0f  

//motor5：M2006 速度环 PID参数以及 PID最大输出，积分输出
#define MOTOR5_SPEED_PID_KP        800.0f
#define MOTOR5_SPEED_PID_KI        0.5f
#define MOTOR5_SPEED_PID_KD        0.0f
#define MOTOR5_SPEED_PID_MAX_OUT   10000.0f
#define MOTOR5_SPEED_PID_MAX_IOUT  8000.0f
//motor5 角度环 角度由编码器 PID参数以及 PID最大输出，积分输出
#define motor5_ENCODE_RELATIVE_PID_KP 15.0f
#define motor5_ENCODE_RELATIVE_PID_KI 0.0f
#define motor5_ENCODE_RELATIVE_PID_KD 0.0f   
#define motor5_ENCODE_RELATIVE_PID_MAX_OUT 40.0f
#define motor5_ENCODE_RELATIVE_PID_MAX_IOUT 0.0f

//motor6：M2006 速度环 PID参数以及 PID最大输出，积分输出
#define MOTOR6_SPEED_PID_KP        800.0f
#define MOTOR6_SPEED_PID_KI        0.5f
#define MOTOR6_SPEED_PID_KD        0.0f
#define MOTOR6_SPEED_PID_MAX_OUT   10000.0f
#define MOTOR6_SPEED_PID_MAX_IOUT  8000.0f
//motor6 角度环 角度由编码器 PID参数以及 PID最大输出，积分输出
#define motor6_ENCODE_RELATIVE_PID_KP 15.0f
#define motor6_ENCODE_RELATIVE_PID_KI 0.0f
#define motor6_ENCODE_RELATIVE_PID_KD 0.0f
#define motor6_ENCODE_RELATIVE_PID_MAX_OUT 40.0f
#define motor6_ENCODE_RELATIVE_PID_MAX_IOUT 0.0f

// //任务初始化 空闲一段时间
#define ARM_TASK_INIT_TIME 201//备用:加入实时操作系统时使用

//电机的控制角度，可以直接由机械臂模型传过来。不使用RM遥控器通道
#define ARM_MODE_CHANNEL 0

//遥控器输入死区，根据实际情况添加死区限制
#define RC_DEADBAND   10

//当电机需要输出反转时，设置下面的值。需要反转时，改变下面值为0
#define JOINT1_TURN  1
#define JOINT2_TURN  1
#define JOINT3_TURN  0
#define JOINT4_TURN  1
#define MOTOR5_TURN  1
#define MOTOR6_TURN  1

//RM电机码盘值最大以及中值
#define HALF_ECD_RANGE  4096
#define ECD_RANGE       8191
//机械臂初始化回中值，允许的误差,并且在误差范围内停止一段时间以及最大时间6s后解除初始化状态，
#define ARM_INIT_ANGLE_ERROR 0.05f
#define ARM_INIT_STOP_TIME 100
#define ARM_INIT_TIME 6000

//机械臂视觉采矿模式初始化角度
#define INIT_JOINT1_SET 0.0f
#define INIT_JOINT2_SET 0.0f
#define INIT_JOINT3_SET 0.0f
#define INIT_JOINT4_SET 0.0f
#define INIT_motor5_SET 0.0f
#define INIT_motor6_SET 0.0f

// //电机编码值转化成角度值
// #ifndef RMMotor_Ecd_to_Rad
// #define RMMotor_Ecd_to_Rad 0.000766990394f // 2*  PI  /8192
// #endif

//电机反馈的转子速度单位由RPM转化为rad/s的比例，1rpm=2πrad/60s=0.10471975511965977rad/s
#define M2006_MOTOR_RPM_TO_SPEED   0.00290888208665721596153948461415f//*0.7272727273f // 0.10471975511965977/36/（11/8）  36是减速电机电机转子与电机输出的减速比
#define M2006_MOTOR_ECD_TO_ANGLE   0.000021305288720633905968306772076277f//*0.7272727273f
#define JOINT5_TO_MOTOR5_6_SCALE  1.375f     //关节5与m2006电机motor5,6的比例，11/8
#define JOINT6_TO_MOTOR5_6_SCALE  0.916666666667f     //关节6与m2006电机motor5,6的比例11/8*2/3


//树宇电机
#define M8010_MOTOR2JOINT_SCALE  3.0f    //joint2的减速比为3：1 
#define M8010_MOTOR_SCALE   6.33f   //宇树电机输出轴与输出转子的减速比6.33
typedef enum
{
    ARM_MOTOR_RAW = 0, //电机原始值控制
    ARM_MOTOR_ENCONDE, //电机编码值角度控制
} arm_motor_mode_e;

typedef struct
{
    fp32 kp;
    fp32 ki;
    fp32 kd;

    fp32 set;
    fp32 get;
    fp32 err;

    fp32 max_out;
    fp32 max_iout;

    fp32 Pout;
    fp32 Iout;
    fp32 Dout;

    fp32 out;
} arm_PID_t;

typedef struct
{
    const motor_measure_t *arm_motor_measure; //RM电机结构体
    arm_PID_t arm_motor_relative_angle_pid;     //相对角度位置pid
    pid_type_def arm_motor_gyro_pid;            //电机 速度环pid
    arm_motor_mode_e arm_motor_mode;            //机械臂电机控制状态机
    arm_motor_mode_e last_arm_motor_mode;       //机械臂上一次控制状态机
    uint16_t offset_ecd;                        //  电机零点偏移量
    int ecd_count;                           //计算电机转子的圈数
    fp32 max_relative_angle; //rad
    fp32 min_relative_angle; //rad
    fp32 relative_angle;     //电机5，6的电机角度
    fp32 relative_angle_set; //rad
    fp32 joint_angel;       //记录关节角度
    fp32 motor_gyro;         //rad/s
    fp32 motor_gyro_set;
    fp32 raw_cmd_current;
    fp32 current_set;
    int16_t given_current;

} arm_rm_motor_t;//RM电机

typedef struct
{
    const DMMotor_measure_t *arm_motor_measure;     //DM电机结构体
    arm_motor_mode_e arm_motor_mode;           //机械臂电机控制状态机
    arm_motor_mode_e last_arm_motor_mode;
    fp32 offset_rad;     //  电机零点偏移量  单位rad
    fp32 max_relative_angle; //rad
    fp32 min_relative_angle; //rad
    fp32 relative_angle;     //rad关节角度
    fp32 relative_angle_set; //rad
    fp32 motor_gyro;         //rad/s
    fp32 motor_gyro_set;  
    fp32 current_set;
    fp32 raw_cmd_current;
    fp32 given_current;

} DM4310_Motor_t;//达妙电机结构体

typedef struct
{
    const MOTOR_recv *arm_motor_measure;     //树宇电机反馈数据结构体
    arm_motor_mode_e arm_motor_mode;           //机械臂电机控制状态机
    arm_motor_mode_e last_arm_motor_mode;
    fp32 offset_rad;     //电机转子偏转位置  单位rad
    fp32 max_relative_angle; //rad
    fp32 min_relative_angle; //rad
    fp32 relative_angle;     //rad关节角度
    fp32 relative_angle_set; //rad
    fp32 motor_gyro;         //rad/s
    fp32 motor_gyro_set;
    fp32 current_set;
    fp32 raw_cmd_current;
    fp32 given_current;

} M8010_Motor_t;//树宇电机结构体

typedef struct
{
    const RC_ctrl_t *arm_rc_ctrl;//遥控器结构体指针
    arm_rm_motor_t arm_motor5_motor;//机械臂motor5(M2006)电机结构体
    arm_rm_motor_t arm_motor6_motor;//机械臂motor6(2006)电机结构体
    DM4310_Motor_t arm_joint1_motor;  //机械臂joint1(DM4310)电机结构体
    DM4310_Motor_t arm_joint4_motor;  //机械臂joint4(DM4310)电机结构体
    M8010_Motor_t  arm_joint2_motor;  //机械臂joint2(树宇M8010)电机结构体
    M8010_Motor_t  arm_joint3_motor;  //机械臂joint3(树宇M8010)电机结构体
} arm_control_t;//机械臂控制结构体


extern const arm_rm_motor_t *get_motor5_motor_point(void);// 返回motor5 电机数据指针
extern const arm_rm_motor_t *get_motor6_motor_point(void);// 返回motor6 电机数据指针
extern const DM4310_Motor_t *get_joint1_motor_point(void); //返回joint1 电机数据指针
extern const DM4310_Motor_t *get_joint4_motor_point(void); //返回joint4 电机数据指针
extern const M8010_Motor_t *get_joint2_motor_point(void); //返回joint2 电机数据指针
extern const M8010_Motor_t *get_joint3_motor_point(void); //返回joint3 电机数据指针

extern void arm_task(void);//后续加入操作系统后调整为下面
// extern void arm_task(void const *pvParameters);

extern const arm_control_t *get_arm_control_point(void);

#endif
